#include "full_cover_path_planner/bcd_decomposer.h"

namespace global_path_planner
{
	namespace ccpp
	{

		BcdDecomposer::BcdDecomposer(std::shared_ptr<Polygon2D> &poly2d)
									: __polygon2d_Ptr(poly2d)
		{
		}

		std::vector<Polygon_2> BcdDecomposer::getPolygonsForBcd(const PolygonWithHoles &polygon_in,
																const Direction_2 &dir)
		{
			// 按照dir旋转多边形方向
			PolygonWithHoles rotated_polygon = rotatePolygonWithHoles(polygon_in, dir);
			// 最外的多边形为逆时针，内部多边形为顺时针
			sortPolygonWithHoles(&rotated_polygon);
			// 构造Simple polygons(移除共线顶点)
			simplifyPolygonWithHoles(&rotated_polygon);
			// 对多边形的顶点进行排序
			std::vector<VertexConstCirculator> sorted_vertices =
				sortXYPolygonWithHoles(rotated_polygon);
			// Initialize edge list.
			std::list<Segment_2> L; // 存储所有待处理的lower、upper边(lower0, upper0, lower1, upper1, ...)
			// 还未处理的polygon
			std::list<Polygon_2> open_polygons;
			// 已经处理过的polygon
			std::vector<Polygon_2> closed_polygons;
			std::vector<Point_2> processed_vertices;

			for (size_t i = 0; i < sorted_vertices.size(); ++i)
			{
				const VertexConstCirculator &v = sorted_vertices[i]; // 可用于遍历v所在多边形的所有顶点
				// v already processed.
				if (std::find(processed_vertices.begin(), processed_vertices.end(), *v) !=
					processed_vertices.end())
					continue;
				processEvent(rotated_polygon, v, &sorted_vertices, &processed_vertices, &L,
							 &open_polygons, &closed_polygons);
			}

			// Rotate back all polygons.
			for (Polygon_2 &p : closed_polygons)
			{
				CGAL::Aff_transformation_2<K> rotation(CGAL::ROTATION, dir, 1, 1e9);
				p = CGAL::transform(rotation, p);
			}

			return closed_polygons;
		}

		void BcdDecomposer::processEvent(const PolygonWithHoles &pwh, const VertexConstCirculator &v,
										 std::vector<VertexConstCirculator> *sorted_vertices,
										 std::vector<Point_2> *processed_vertices,
										 std::list<Segment_2> *L, std::list<Polygon_2> *open_polygons,
										 std::vector<Polygon_2> *closed_polygons)
		{
			// Compute intersection.
			Line_2 l(*v, Direction_2(0, 1)); // l: 待处理顶点对应的线扫直线
			std::vector<Point_2> intersections = getSegmentIntersections(*L, l); // l和L的所有交点 

			// Get e_lower and e_upper.
			Segment_2 e_prev(*v, *std::prev(v));
			Segment_2 e_next(*v, *std::next(v));
			// Catch vertical edges.
			Polygon_2::Traits::Equal_x_2 eq_x_2;
			if (eq_x_2(e_prev.source(), e_prev.target()))
			{
				e_prev = Segment_2(*std::prev(v), *std::prev(v, 2));
			}
			else if (eq_x_2(e_next.source(), e_next.target()))
			{
				e_next = Segment_2(*std::next(v), *std::next(v, 2));
			}

			Polygon_2::Traits::Less_y_2 less_y_2;
			Polygon_2::Traits::Less_x_2 less_x_2;
			Segment_2 e_lower = e_prev;
			Segment_2 e_upper = e_next;
			if (less_x_2(e_prev.target(), e_prev.source()) &&
				less_x_2(e_next.target(), e_next.source()))
			{
				// out
				outEvent(e_lower, e_upper, intersections, pwh, v, sorted_vertices,
						 processed_vertices, L, open_polygons, closed_polygons);
			}
			else if (!less_x_2(e_lower.target(), e_lower.source()) &&
					 !less_x_2(e_upper.target(), e_upper.source()))
			{
				// in
				inEvent(e_lower, e_upper, intersections, pwh, v, sorted_vertices,
						processed_vertices, L, open_polygons, closed_polygons);
			}
			else
			{
				// other
				otherEvent(v, processed_vertices, sorted_vertices, L, open_polygons);
			}
		}

		// e_lower，e_upper位于V顶点左边
		void BcdDecomposer::outEvent(Segment_2 &e_lower, Segment_2 &e_upper,
									 std::vector<Point_2> &intersections,
									 const PolygonWithHoles &pwh, const VertexConstCirculator &v,
									 std::vector<VertexConstCirculator> *sorted_vertices,
									 std::vector<Point_2> *processed_vertices,
									 std::list<Segment_2> *L, std::list<Polygon_2> *open_polygons,
									 std::vector<Polygon_2> *closed_polygons)
		{
			Polygon_2::Traits::Equal_2 eq_2;
			Point_2 p_on_upper = eq_2(e_lower.source(), e_upper.source())
									 ? e_upper.target()
									 : e_upper.source();
			// 直线的右边为负，左边为正
			if (e_lower.supporting_line().has_on_positive_side(p_on_upper))
				std::swap(e_lower, e_upper);

			// Determine whether we close one or close two and open one.
			// TODO(rikba): instead of looking at unbounded side, look at adjacent edge
			// angle
			bool close_one = outOfPWH(pwh, *v + Vector_2(1e-6, 0));
			// Find edges to remove.
			std::list<Segment_2>::iterator e_lower_it = L->begin();
			size_t e_lower_id = 0;
			for (; e_lower_it != L->end(); ++e_lower_it)
			{
				if (*e_lower_it == e_lower || *e_lower_it == e_lower.opposite())
				{
					break;
				}
				e_lower_id++;
			}

			std::list<Segment_2>::iterator e_upper_it = std::next(e_lower_it);
			size_t e_upper_id = e_lower_id + 1;
			size_t lower_cell_id = e_lower_id / 2;
			size_t upper_cell_id = e_upper_id / 2;

			if (close_one)
			{
				std::list<Polygon_2>::iterator cell =
					std::next(open_polygons->begin(), lower_cell_id);

				cell->push_back(e_lower.source());
				if (!eq_2(e_lower.source(), e_upper.source()))
				{
					cell->push_back(e_upper.source());
				}
				if (__polygon2d_Ptr->createPolygon2(&*cell))
					closed_polygons->push_back(*cell);

				L->erase(e_lower_it);
				L->erase(e_upper_it);

				open_polygons->erase(cell);
			}
			else
			{
				// Close two cells, open one.
				// Close lower cell.
				std::list<Polygon_2>::iterator lower_cell =
					std::next(open_polygons->begin(), lower_cell_id);

				lower_cell->push_back(intersections[e_lower_id - 1]);
				lower_cell->push_back(intersections[e_lower_id]);
				if (__polygon2d_Ptr->createPolygon2(&*lower_cell))
					closed_polygons->push_back(*lower_cell);
				// Close upper cell.
				std::list<Polygon_2>::iterator upper_cell =
					std::next(open_polygons->begin(), upper_cell_id);

				upper_cell->push_back(intersections[e_upper_id]);
				upper_cell->push_back(intersections[e_upper_id + 1]);
				if (__polygon2d_Ptr->createPolygon2(&*upper_cell))
					closed_polygons->push_back(*upper_cell);

				// Delete e_lower and e_upper from list.
				L->erase(e_lower_it);
				L->erase(e_upper_it);

				// Open one new cell.
				std::list<Polygon_2>::iterator new_polygon =
					open_polygons->insert(lower_cell, Polygon_2());
				new_polygon->push_back(intersections[e_upper_id + 1]);
				new_polygon->push_back(intersections[e_lower_id - 1]);
				open_polygons->erase(lower_cell);
				open_polygons->erase(upper_cell);
			}
			processed_vertices->push_back(e_lower.source());
			if (!eq_2(e_lower.source(), e_upper.source()))
			{
				processed_vertices->push_back(e_upper.source());
			}
		}

		// e_lower，e_upper位于V顶点右边
		void BcdDecomposer::inEvent(Segment_2 &e_lower, Segment_2 &e_upper,
									std::vector<Point_2> &intersections,
									const PolygonWithHoles &pwh, const VertexConstCirculator &v,
									std::vector<VertexConstCirculator> *sorted_vertices,
									std::vector<Point_2> *processed_vertices,
									std::list<Segment_2> *L, std::list<Polygon_2> *open_polygons,
									std::vector<Polygon_2> *closed_polygons)
		{
			Polygon_2::Traits::Equal_2 eq_2;
			Polygon_2::Traits::Less_y_2 less_y_2;
			Polygon_2::Traits::Less_x_2 less_x_2;
			Point_2 p_on_lower = eq_2(e_lower.source(), e_upper.source())
									 ? e_lower.target()
									 : e_lower.source();
			if (e_upper.supporting_line().has_on_positive_side(p_on_lower))
				std::swap(e_lower, e_upper);

			// Determine whether we open one or close one and open two.
			bool open_one = outOfPWH(pwh, *v - Vector_2(1e-6, 0));

			// Find edge to update.
			size_t e_LOWER_id = 0;
			bool found_e_lower_id = false;
			for (size_t i = 0; i < intersections.size() - 1; i = i + 2)
			{
				if (intersections.empty())
					break;
				if (open_one)
				{
					if (less_y_2(intersections[i], e_lower.source()) &&
						less_y_2(intersections[i + 1], e_upper.source()))
					{
						e_LOWER_id = i;
						found_e_lower_id = true;
					}
				}
				else
				{
					if (less_y_2(intersections[i], e_lower.source()) &&
						less_y_2(e_upper.source(), intersections[i + 1]))
					{
						e_LOWER_id = i;
					}
				}
			}
			if (open_one)
			{
				// Add one new cell above e_UPPER.
				std::list<Segment_2>::iterator e_UPPER = L->begin();
				std::list<Polygon_2>::iterator open_cell = open_polygons->begin();
				if (!L->empty() && found_e_lower_id)
				{
					e_UPPER = std::next(e_UPPER, e_LOWER_id + 1);
					open_cell = std::next(open_cell, e_LOWER_id / 2 + 1);
				}
				// Update edge list.
				if (L->empty())
				{
					L->insert(L->end(), e_lower);
					L->insert(L->end(), e_upper);
				}
				else if (!L->empty() && !found_e_lower_id)
				{
					L->insert(L->begin(), e_upper);
					L->insert(L->begin(), e_lower);
				}
				else
				{
					std::list<Segment_2>::iterator inserter = std::next(e_UPPER);
					L->insert(inserter, e_lower);
					L->insert(inserter, e_upper);
				}

				// Create new polygon.
				std::list<Polygon_2>::iterator open_polygon =
					open_polygons->insert(open_cell, Polygon_2());
				open_polygon->push_back(e_upper.source());
				if (!eq_2(e_lower.source(), e_upper.source()))
				{
					open_polygon->push_back(e_lower.source());
				}
			}
			else
			{
				// Add new polygon between e_LOWER and e_UPPER.
				std::list<Segment_2>::iterator e_LOWER =
					std::next(L->begin(), e_LOWER_id);
				std::list<Polygon_2>::iterator cell =
					std::next(open_polygons->begin(), e_LOWER_id / 2);

				// Add e_lower and e_upper
				std::list<Segment_2>::iterator e_lower_it =
					L->insert(std::next(e_LOWER), e_lower);
				L->insert(std::next(e_lower_it), e_upper);

				// Add new cell.
				std::list<Polygon_2>::iterator new_polygon =
					open_polygons->insert(cell, Polygon_2());

				// Close one cell.
				cell->push_back(intersections[e_LOWER_id]);
				cell->push_back(intersections[e_LOWER_id + 1]);
				if (__polygon2d_Ptr->createPolygon2(&*cell))
					closed_polygons->push_back(*cell);
				// Open two new cells
				// Lower polygon.
				new_polygon->push_back(e_lower.source());
				new_polygon->push_back(intersections[e_LOWER_id]);

				// Upper polygon.
				new_polygon = open_polygons->insert(cell, Polygon_2());
				new_polygon->push_back(intersections[e_LOWER_id + 1]);
				new_polygon->push_back(e_upper.source());
				// Close old cell.
				open_polygons->erase(cell);
			}
			processed_vertices->push_back(e_lower.source());
			if (!eq_2(e_lower.source(), e_upper.source()))
			{
				processed_vertices->push_back(e_upper.source());
			}
		}

		// 顶点位于e_lower和e_upper中间
		void BcdDecomposer::otherEvent(const VertexConstCirculator &v,
									   std::vector<Point_2> *processed_vertices,
									   std::vector<VertexConstCirculator> *sorted_vertices,
									   std::list<Segment_2> *L, std::list<Polygon_2> *open_polygons)
		{
			// TODO(rikba): Sort vertices correctly in the first place.
			// Check if v exits among edges.
			VertexConstCirculator v_middle = v;
			Polygon_2::Traits::Equal_2 eq_2;
			std::list<Segment_2>::iterator it = L->end();
			while (it == L->end())
			{
				for (it = L->begin(); it != L->end(); it++)
				{
					if (*v_middle == it->source() || *v_middle == it->target())
					{
						// Swap v in sorted vertices.
						if (!eq_2(*v, *v_middle))
						{ // 处理内部外部多边形交叉, 当前不存在这种情况
							std::vector<VertexConstCirculator>::iterator i_v =
								sorted_vertices->end();
							std::vector<VertexConstCirculator>::iterator i_v_middle =
								sorted_vertices->end();
							for (std::vector<VertexConstCirculator>::iterator it =
									 sorted_vertices->begin();
								 it != sorted_vertices->end(); ++it)
							{
								if (*it == v)
									i_v = it;
								if (*it == v_middle)
									i_v_middle = it;
							}

							std::iter_swap(i_v, i_v_middle);
						}
						break;
					}
				}
				if (it == L->end())
				{
					VertexConstCirculator v_prev = std::prev(v_middle);
					VertexConstCirculator v_next = std::next(v_middle);

					if (v_prev->x() == v_middle->x())
						v_middle = v_prev;
					else
						v_middle = v_next;

					// v_middle = v_next;
					// if(v_middle->x() == v_tmp_middle->x() &&
					//   v_middle->y() == v_tmp_middle->y()) break;
				}
			}

			// Correct vertical edges.
			auto e_prev = Segment_2(*v_middle, *std::prev(v_middle));
			auto e_next = Segment_2(*v_middle, *std::next(v_middle));

			// Find edge to update.
			std::list<Segment_2>::iterator old_e_it = L->begin();
			Segment_2 new_edge;
			size_t edge_id = 0;
			for (; old_e_it != L->end(); ++old_e_it)
			{
				if (*old_e_it == e_next || *old_e_it == e_next.opposite())
				{
					new_edge = e_prev;
					break;
				}
				else if (*old_e_it == e_prev || *old_e_it == e_prev.opposite())
				{
					new_edge = e_next;
					break;
				}
				edge_id++;
			}

			// Update cell with new vertex.
			size_t cell_id = edge_id / 2;
			std::list<Polygon_2>::iterator cell =
				std::next(open_polygons->begin(), cell_id);

			if ((edge_id % 2) == 0)
			{
				// Case 1: Insert new vertex at end.
				cell->push_back(new_edge.source());
			}
			else
			{
				// Case 2: Insert new vertex at begin.
				cell->insert(cell->vertices_begin(), new_edge.source());
			}
			// Update edge.
			// if(old_e_it != L->end()){
			L->insert(old_e_it, new_edge);
			L->erase(old_e_it);
			// }

			processed_vertices->push_back(*v_middle);
		}

		// 多边形旋转方向
		PolygonWithHoles BcdDecomposer::rotatePolygonWithHoles(const PolygonWithHoles &pwh,
															   const Direction_2 &dir)
		{
    		PolygonWithHoles rotated_polygon = pwh;		
    		rotated_polygon.outer_boundary() =
    		            __polygon2d_Ptr->rotatePolygon(pwh.outer_boundary(), dir);

    		PolygonWithHoles::Hole_iterator hit_rot = rotated_polygon.holes_begin();
    		for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin();
    		     hit != pwh.holes_end(); ++hit) {
    		  *(hit_rot++) = __polygon2d_Ptr->rotatePolygon(*hit, dir);;
    		}

    		return rotated_polygon;
		}

		// 获取两条直线或者直线与段之间的交叉点
		std::vector<Point_2> BcdDecomposer::getSegmentIntersections(const std::list<Segment_2> &L,
																	const Line_2 &l)
		{
			typedef CGAL::cpp11::result_of<Intersect_2(Segment_2, Line_2)>::type
				Intersection;

			std::vector<Point_2> intersections(L.size());
			std::vector<Point_2>::iterator intersection = intersections.begin();
			for (std::list<Segment_2>::const_iterator it = L.begin(); it != L.end();
				 ++it)
			{
				Intersection result = CGAL::intersection(*it, l);
				if (result)
				{
					if (boost::get<Segment_2>(&*result))
					{
						*(intersection++) = it->target();
					}
					else
					{
						const Point_2 *p = boost::get<Point_2>(&*result);
						*(intersection++) = *p;
					}
				}
				else
				{
					std::cout << "No intersection found!" << std::endl;
				}
			}

			return intersections;
		}

		// 最外的多边形为逆时针，内部多边形为顺时针
		void BcdDecomposer::sortPolygonWithHoles(PolygonWithHoles *pwh)
		{
			// TODO -- 整合到多边形
			if (pwh->outer_boundary().is_clockwise_oriented())
				pwh->outer_boundary().reverse_orientation();

			for (PolygonWithHoles::Hole_iterator hi = pwh->holes_begin();
				 hi != pwh->holes_end(); ++hi)
				if (hi->is_counterclockwise_oriented())
					hi->reverse_orientation();
		}

		// 对多边形的顶点VertexConstCirculator进行排序
		// 1.若x不相同，按照小的排序；2.若x相同，按照y小的排序
		std::vector<VertexConstCirculator> BcdDecomposer::sortXYPolygonWithHoles(const PolygonWithHoles &pwh)
		{
			// TODO -- 整合到多边形中
			std::vector<VertexConstCirculator> sorted_vertices;
			// Get boundary vertices.
			VertexConstCirculator v = pwh.outer_boundary().vertices_circulator();
			do
			{
				sorted_vertices.push_back(v);
			} while (++v != pwh.outer_boundary().vertices_circulator());
			// Get hole vertices.
			for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin();
				 hit != pwh.holes_end(); ++hit)
			{
				VertexConstCirculator vh = hit->vertices_circulator();
				do
				{
					sorted_vertices.push_back(vh);
				} while (++vh != hit->vertices_circulator());
			}
			__polygon2d_Ptr->sortVertices(sorted_vertices);

			return sorted_vertices;
		}

		// 1.计算点p是不是在多边形的边上
		// 2.若不在，判断点p是否在hole的边上
		bool BcdDecomposer::outOfPWH(const PolygonWithHoles &pwh, const Point_2 &p)
		{
			// TODO -- 整合到多边形类中
			if (pwh.outer_boundary().has_on_unbounded_side(p))
				return true;

			for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin();
				 hit != pwh.holes_end(); ++hit)
			{
				if (hit->has_on_bounded_side(p))
				{
					return true;
				}
			}
			return false;
		}

		// 判断PolygonWithHoles几何模型是不是Simple
		bool BcdDecomposer::isStrictlySimple(const PolygonWithHoles &pwh)
		{
			for (PolygonWithHoles::Hole_const_iterator hi = pwh.holes_begin();
				 hi != pwh.holes_end(); ++hi)
				if (!__polygon2d_Ptr->isStrictlySimple(*hi))
					return false;
			return __polygon2d_Ptr->isStrictlySimple(pwh.outer_boundary());
		}

		// 移除多边形共线的顶点
		void BcdDecomposer::simplifyPolygonWithHoles(PolygonWithHoles *pwh)
		{
			__polygon2d_Ptr->removeCollinearVertices(&pwh->outer_boundary());
			for (auto hi = pwh->holes_begin(); hi != pwh->holes_end(); ++hi)
				__polygon2d_Ptr->removeCollinearVertices(&*hi);
		}

	} // namespace ccpp
} // namespace global_path_planner